/** 
 * @file HMC58X3.c
 * 
 * @detail Description : 
 * Magnetometer function calls to interface a MSP430F5xx via I2C with a HMC58x3  
 * 
 * @author E. Macias / D. Torres / S. Ravindran
 * @author Texas Instruments, Inc
 * @date December, 2011
 * @version 1.0 - Initial version
 * @note Built with IAR Embedded Workbench: 5.20.1 and CCS Version 5.1.0.09000
 
 * Based on:
 * HMC58X3.cpp - Interface a Honeywell HMC58X3 or HMC5883L magnetometer to an Arduino via i2c

 * Author: Fabio Varesano <fvaresano@yahoo.it>
 * Copyright (C) 2011 Fabio Varesano <fvaresano@yahoo.it>

 * Based on:
 * http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1274748346
 *   Modification/extension of the following by E.J.Muller
 * http://eclecti.cc/hardware/hmc5843-magnetometer-library-for-arduino
 *   Copyright (c) 2009 Nirav Patel, 

 * The above were based on:
 * http://www.sparkfun.com/datasheets/Sensors/Magneto/HMC58X3-v11.c
 * http://www.atmel.com/dyn/resources/prod_documents/doc2545.pdf

 * Copyright 2011 Fabio Varesano - www.varesano.net

 * This work is licensed under the Creative Commons Attribution 3.0 United States License. 
 * To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/ or 
 * send a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.

*/

#include "HMC58x3.h"

float x_scale,y_scale,z_scale;


void getValues_int(int16_t *x,int16_t *y,int16_t *z) {
  float fx,fy,fz;
  getValues_float(&fx,&fy,&fz);
  *x= (int16_t) (fx + 0.5);
  *y= (int16_t) (fy + 0.5);
  *z= (int16_t) (fz + 0.5);
}

/**
* @brief <b>Function Name</b>:     : getValues_float                                               
* @brief  <b>Description</b>: This function calls function getRaw(), and uses
* the scale value for each axes calculated in the calibrate(), to calculate 
* the correct x, y, and z float value. 
* @param Input Parameters: 
*<BR>float *x is the location where the x axis value is stored.
*<BR>float *y is the location where the y axis value is stored.
*<BR>float *z is the location where the z axis value is stored.
* @return Return Values: None                                                    
**/ 
void getValues_float(float *x,float *y,float *z) {
  int16_t xr,yr,zr;
  
  getRaw(&xr, &yr, &zr);
  *x= ((float) xr) / x_scale;
  *y = ((float) yr) / y_scale;
  *z = ((float) zr) / z_scale;
}

void getValues(float *xyz) {
  getValues_float(&xyz[0], &xyz[1], &xyz[2]);
}

/**
* @brief <b>Function Name</b>:     : getRaw                                               
* @brief  <b>Description</b>: This function reads the magnetometer's three axes
* and stores the values in x, y, and z pointer address. This funciton requires
* the correct magnetomter to be defined in HMC58x3.h.
* @param Input Parameters: 
*<BR>int16_t *x is the location where the x axis value is stored.
*<BR>int16_t *y is the location where the y axis value is stored.
*<BR>int16_t *z is the location where the z axis value is stored.
* @return Return Values: None                                                    
**/ 
void getRaw(int16_t *x,int16_t *y,int16_t *z) {
  uint8_t rx_temp_buffer[6];
  
  // Magnetometer
  I2C_Set_Slave_Address(0x1E);
  
  rx_temp_buffer[0] = HMC58X3_R_XM;
  I2C_Read_Packet_From_Sensor(&rx_temp_buffer[0],6);
   
  // read out the 3 values, 2 bytes each.
  *x = (rx_temp_buffer[0] << 8) | rx_temp_buffer[1];
  #ifdef ISHMC5843
    *y = (rx_temp_buffer[2] << 8) | rx_temp_buffer[3];
    *z = (rx_temp_buffer[4] << 8) | rx_temp_buffer[5];
  #else // the Z registers comes before the Y registers in the HMC5883L
    *z = (rx_temp_buffer[2] << 8) | rx_temp_buffer[3];
    *y = (rx_temp_buffer[4] << 8) | rx_temp_buffer[5];
  #endif
  // the HMC58X3 will automatically wrap around on the next request
}

/**
* @brief <b>Function Name</b>:     : calibrate                                               
* @brief  <b>Description</b>: This function calibrates the magnetomter, by reading
* 10 values for each x, y, and z axes. Meanwhile it finds which is the maximum
* value, and calculates the scale for each axes. The scale is used in the 
* Read_Compass() inside Magnetometer.c. 
* @param Input Parameters: 
*<BR>int16_t gain is the gain value to store in the magnetometer's gain register.
* @return Return Values: None                                                    
**/ 
void calibrate(uint16_t gain)
{
  // Magnetometer Address
  I2C_Set_Slave_Address(HMC58X3_ADDR);
   
  volatile uint16_t i;
  float x, y, z, mx=0, my=0, mz=0, t=10;
  float max=0;
  
  x_scale=1; // get actual values
  y_scale=1;
  z_scale=1;
  
  global_tx_buffer[0] = HMC58X3_R_CONFA; // Register
  global_tx_buffer[1] = (0x10+HMC_POS_BIAS); 	
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  
  // Set Gain
  if(gain < 7)
  {
  	global_tx_buffer[0] = HMC58X3_R_CONFB; // Register
  	global_tx_buffer[1] = (gain << 5); 	
  	I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  }
  
  // Read 10 values for all 3 axes, and calculate the maximum value.
  for (i=0; i<(uint16_t)t; i++) { 
    global_tx_buffer[0] = HMC58X3_R_MODE; // Register
    global_tx_buffer[1] = 0x01; // Set Mode to 0x01
    I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
    Sleep(100);
    getValues_float(&x,&y,&z);
    if (x>mx) mx=x;
    if (y>my) my=y;
    if (z>mz) mz=z;
  }
  
  if (mx>max) max=mx;
  if (my>max) max=my;
  if (mz>max) max=mz;
  x_scale=max/mx; // calc scales
  y_scale=max/my;
  z_scale=max/mz;
  //writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default
  global_tx_buffer[0] = HMC58X3_R_CONFA; // Register

  global_tx_buffer[1] = 0x70; 	
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
}
